#include <stdio.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <errno.h>
#include <stdlib.h>
#include <signal.h>
#include <poll.h>
#include <string.h>
#include <linux/videodev2.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/types_c.h>
#include "opencv_demo.h"

static bool quit = false;

using namespace std;
#if 0
static void
print_usage(void) {
    printf("\n\tUsage: 4cameras_egl_demo [OPTIONS]\n\n"
           "\tExample: \n"
           "\t./4cameras_egl_demo -d /dev/video0 -s 1280x720\n\n"
           "\tSupported options:\n"
           "\t-d\t\tSet V4l2 video device node\n"
           "\t-s\t\tSet output resolution of video device\n"
           "\t-f\t\tSet output pixel format of video device (supports only YUYV/YVYU/UYVY/VYUY/GREY/MJPEG)\n"
           "\t-r\t\tSet renderer frame rate (30 fps by default)\n"
           "\t-n\t\tSave the n-th frame before VIC processing\n"
           "\t-c\t\tEnable CUDA aglorithm (draw a black box in the upper left corner)\n"
           "\t-v\t\tEnable verbose message\n"
           "\t-h\t\tPrint this usage\n\n"
           "\tNOTE: It runs infinitely until you terminate it with <ctrl+c>\n");
}

static bool
parse_cmdline(context_t *ctx, int argc, char **argv) {
    int c;

    if (argc < 2) {
        print_usage();
        exit(EXIT_SUCCESS);
    }

    while ((c = getopt(argc, argv, "d:s:f:r:n:cvh")) != -1) {
        switch (c) {
            case 'd':
                ctx->cam_devname = optarg;
                break;
            case 's':
                if (sscanf(optarg, "%dx%d",
                           &ctx->cam_w, &ctx->cam_h) != 2) {
                    print_usage();
                    return false;
                }
                break;
            case 'f':
                break;
            case 'r':
                ctx->fps = strtol(optarg, NULL, 10);
                break;
            case 'n':
                ctx->save_n_frame = strtol(optarg, NULL, 10);
                break;
            case 'c':
                break;
            case 'v':
                ctx->enable_verbose = true;
                break;
            case 'h':
                print_usage();
                exit(EXIT_SUCCESS);
                break;
            default:
                print_usage();
                return false;
        }
    }

    return true;
}
#endif
static bool
parse_cmdline(context_t *ctx, int argc, char **argv) {
    int c;

    while ((c = getopt(argc, argv, "d:s:f:r:n:cvh")) != -1) {
        switch (c) {
            case 'd':
                ctx->cam_devname = optarg;
                break;
            case 's':
                if (sscanf(optarg, "%dx%d",
                           &ctx->cam_w, &ctx->cam_h) != 2) {
                    return false;
                }
                break;
	   default:
		return false;
	    }

	}
    return true;
}

static void
set_defaults(context_t *ctx) {
    memset(ctx, 0, sizeof(context_t));

    ctx->cam_devname = "/dev/video0";
    ctx->cam_fd = -1;
    ctx->cam_pixfmt = V4L2_PIX_FMT_UYVY;
    ctx->cam_w = 640;
    ctx->cam_h = 480;
    ctx->frame = 0;
    ctx->save_n_frame = 0;

    ctx->g_buff = NULL;
    ctx->capture_dmabuf = false;

    ctx->enable_verbose = false;
}

static bool
camera_initialize(context_t *ctx) {
    struct v4l2_format fmt;
    INFO("cam_devname: %s", ctx->cam_devname);
    // Open camera device
    ctx->cam_fd = open(ctx->cam_devname, O_RDWR);
    if (ctx->cam_fd == -1)
        ERROR_RETURN("Failed to open camera device %s: %s (%d)",
                     ctx->cam_devname, strerror(errno), errno);

    // Set camera output format
    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = ctx->cam_w;
    fmt.fmt.pix.height = ctx->cam_h;
    fmt.fmt.pix.pixelformat = ctx->cam_pixfmt;
    fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
    if (ioctl(ctx->cam_fd, VIDIOC_S_FMT, &fmt) < 0)
        ERROR_RETURN("Failed to set camera output format: %s (%d)",
                     strerror(errno), errno);

    // Get the real format in case the desired is not supported
    memset(&fmt, 0, sizeof fmt);
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(ctx->cam_fd, VIDIOC_G_FMT, &fmt) < 0)
        ERROR_RETURN("Failed to get camera output format: %s (%d)",
                     strerror(errno), errno);
    if (fmt.fmt.pix.width != ctx->cam_w ||
        fmt.fmt.pix.height != ctx->cam_h ||
        fmt.fmt.pix.pixelformat != ctx->cam_pixfmt) {
        ctx->cam_w = fmt.fmt.pix.width;
        ctx->cam_h = fmt.fmt.pix.height;
        ctx->cam_pixfmt = fmt.fmt.pix.pixelformat;
    }

    struct v4l2_streamparm streamparm;
    memset (&streamparm, 0x00, sizeof (struct v4l2_streamparm));
    streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ioctl (ctx->cam_fd, VIDIOC_G_PARM, &streamparm);

    INFO("Camera ouput format: (%d x %d)  stride: %d, imagesize: %d, frate: %u / %u",
         fmt.fmt.pix.width,
         fmt.fmt.pix.height,
         fmt.fmt.pix.bytesperline,
            fmt.fmt.pix.sizeimage,
            streamparm.parm.capture.timeperframe.denominator,
            streamparm.parm.capture.timeperframe.numerator);

    return true;
}

static bool
init_components(context_t *ctx) {
    if (!camera_initialize(ctx))
        ERROR_RETURN("Failed to initialize camera device");
    INFO("Initialize v4l2 components successfully");
    return true;
}

static bool
request_camera_buff(context_t *ctx) {
    // Request camera v4l2 buffer
    struct v4l2_requestbuffers rb;
    memset(&rb, 0, sizeof(rb));
    rb.count = V4L2_BUFFERS_NUM;
    rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    rb.memory = V4L2_MEMORY_DMABUF;
    if (ioctl(ctx->cam_fd, VIDIOC_REQBUFS, &rb) < 0)
        ERROR_RETURN("Failed to request v4l2 buffers: %s (%d)",
                     strerror(errno), errno);
    if (rb.count != V4L2_BUFFERS_NUM)
        ERROR_RETURN("V4l2 buffer number is not as desired");

    for (unsigned int index = 0; index < V4L2_BUFFERS_NUM; index++) {
        struct v4l2_buffer buf;

        // Query camera v4l2 buf length
        memset(&buf, 0, sizeof buf);
        buf.index = index;
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_DMABUF;

        if (ioctl(ctx->cam_fd, VIDIOC_QUERYBUF, &buf) < 0)
            ERROR_RETURN("Failed to query buff: %s (%d)",
                         strerror(errno), errno);

        // TODO add support for multi-planer
        // Enqueue empty v4l2 buff into camera capture plane
        buf.m.fd = (unsigned long) ctx->g_buff[index].dmabuff_fd;
        if (buf.length != ctx->g_buff[index].size) {
            WARN("Camera v4l2 buf length is not expected");
            ctx->g_buff[index].size = buf.length;
        }

        if (ioctl(ctx->cam_fd, VIDIOC_QBUF, &buf) < 0)
            ERROR_RETURN("Failed to enqueue buffers: %s (%d)",
                         strerror(errno), errno);
    }

    return true;
}

static bool
request_camera_buff_mmap(context_t *ctx) {
    // Request camera v4l2 buffer
    struct v4l2_requestbuffers rb;
    memset(&rb, 0, sizeof(rb));
    rb.count = V4L2_BUFFERS_NUM;
    rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    rb.memory = V4L2_MEMORY_MMAP;
    if (ioctl(ctx->cam_fd, VIDIOC_REQBUFS, &rb) < 0)
        ERROR_RETURN("Failed to request v4l2 buffers: %s (%d)",
                     strerror(errno), errno);
    if (rb.count != V4L2_BUFFERS_NUM)
        ERROR_RETURN("V4l2 buffer number is not as desired");

    for (unsigned int index = 0; index < V4L2_BUFFERS_NUM; index++) {
        struct v4l2_buffer buf;

        // Query camera v4l2 buf length
        memset(&buf, 0, sizeof buf);
        buf.index = index;
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

        buf.memory = V4L2_MEMORY_MMAP;
        if (ioctl(ctx->cam_fd, VIDIOC_QUERYBUF, &buf) < 0)
            ERROR_RETURN("Failed to query buff: %s (%d)",
                         strerror(errno), errno);

        ctx->g_buff[index].size = buf.length;
        ctx->g_buff[index].start = (unsigned char *)
                mmap(NULL /* start anywhere */,
                     buf.length,
                     PROT_READ | PROT_WRITE /* required */,
                     MAP_SHARED /* recommended */,
                     ctx->cam_fd, buf.m.offset);
        if (MAP_FAILED == ctx->g_buff[index].start)
            ERROR_RETURN("Failed to map buffers");

        if (ioctl(ctx->cam_fd, VIDIOC_QBUF, &buf) < 0)
            ERROR_RETURN("Failed to enqueue buffers: %s (%d)",
                         strerror(errno), errno);
    }

    return true;
}


static bool
prepare_buffers(context_t *ctx) {

    // Allocate global buffer context
    ctx->g_buff = (nv_buffer *) malloc(V4L2_BUFFERS_NUM * sizeof(nv_buffer));
    if (ctx->g_buff == NULL)
        ERROR_RETURN("Failed to allocate global buffer context");

    if (ctx->capture_dmabuf) {
        if (!request_camera_buff(ctx))
            ERROR_RETURN("Failed to set up camera buff");
    } else {
        if (!request_camera_buff_mmap(ctx))
            ERROR_RETURN("Failed to set up camera buff");
    }

    INFO("Succeed in preparing stream buffers");
    return true;
}

static bool
start_stream(context_t *ctx) {
    enum v4l2_buf_type type;

    // Start v4l2 streaming
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(ctx->cam_fd, VIDIOC_STREAMON, &type) < 0)
        ERROR_RETURN("Failed to start streaming: %s (%d)",
                     strerror(errno), errno);

    usleep(200);

    INFO("Camera video streaming on ...");
    return true;
}

static void
signal_handle(int signum) {
    printf("Quit due to exit command from user!\n");
    quit = true;
}

static bool
stop_stream(context_t *ctx) {
    enum v4l2_buf_type type;

    // Stop v4l2 streaming
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(ctx->cam_fd, VIDIOC_STREAMOFF, &type))
        ERROR_RETURN("Failed to stop streaming: %s (%d)",
                     strerror(errno), errno);

    return true;
}

static bool
start_capture(context_t *ctx) {
    struct sigaction sig_action;
    struct pollfd fds[1];
    cv::Mat imgbuf;

    // Ensure a clean shutdown if user types <ctrl+c>
    sig_action.sa_handler = signal_handle;
    sigemptyset(&sig_action.sa_mask);
    sig_action.sa_flags = 0;
    sigaction(SIGINT, &sig_action, NULL);

    fds[0].fd = ctx->cam_fd;
    fds[0].events = POLLIN;

    while (poll(fds, 1, 5000) > 0 && !quit) {
        if (fds[0].revents & POLLIN) {
            struct v4l2_buffer v4l2_buf;

            // Dequeue camera buff
            memset(&v4l2_buf, 0, sizeof(v4l2_buf));
            v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            if (ctx->capture_dmabuf)
                v4l2_buf.memory = V4L2_MEMORY_DMABUF;
            else
                v4l2_buf.memory = V4L2_MEMORY_MMAP;
            if (ioctl(ctx->cam_fd, VIDIOC_DQBUF, &v4l2_buf) < 0)
                ERROR_RETURN("Failed to dequeue camera buff: %s (%d)",
                             strerror(errno), errno);

            ctx->frame++;
            printf("frame No : %d\n", ctx->frame);
            printf("frame size : %d\n", v4l2_buf.length);


            uchar *b_Buffer = new uchar[ctx->cam_w*ctx->cam_h*2];
            memcpy(b_Buffer, ctx->g_buff[v4l2_buf.index].start, v4l2_buf.length);
#if 0
            cv::Mat mSrc_RGB(szSize, CV_8UC3);
            cvtColor(mSrc, mSrc_RGB, CV_YUV2RGB_YVYU);
            imshow("Image1 RGB", mSrc_RGB);
#else
//	    cv::Mat  imgbuf1 = cv::Mat(720, 1280, CV_8UC2, b_Buffer);
//	    cv::Mat imgbuf(480, 640 cv_8UC2);
//	    resize(imgbuf1, imgbuf, Size(480, 640), inter );
  //          cv::Mat mrgba(480, 640, CV_8UC3);
    //        cv::cvtColor(imgbuf, mrgba, cv::COLOR_YUV2BGR_UYVY);
      //      cv::imshow("camera 0", mrgba);

           cv::Size szSize(ctx->cam_w, ctx->cam_h);
           cv::Mat mSrc(szSize, CV_8UC2, b_Buffer);

           cv::Mat mSrc_BGR(szSize, CV_8UC3);
	 
           cvtColor(mSrc, mSrc_BGR, CV_YUV2BGR_UYVY);

           cv::Mat smSrc_BGR(ctx->cam_h / 2, ctx->cam_w /2 , CV_8UC3);
	   resize(mSrc_BGR, smSrc_BGR, smSrc_BGR.size());
           imshow("camera 0", smSrc_BGR);

#endif
            cv::waitKey(1); // Wait for 'esc' key press to exit
            delete[] b_Buffer;

            if (ioctl(ctx->cam_fd, VIDIOC_QBUF, &v4l2_buf))
                    ERROR_RETURN("Failed to queue camera buffers: %s (%d)",
                                strerror(errno), errno);
            }
    }
    return true;
}


int
main(int argc, char *argv[]) {
    context_t ctx;
    int error = 0;

    set_defaults(&ctx);

    CHECK_ERROR(parse_cmdline(&ctx, argc, argv), cleanup,
                "Invalid options specified");

    CHECK_ERROR(init_components(&ctx), cleanup,
                "Failed to initialize v4l2 components");

    CHECK_ERROR(prepare_buffers(&ctx), cleanup,
                "Failed to prepare v4l2 buffs");

    CHECK_ERROR(start_stream(&ctx), cleanup,
                "Failed to start streaming");

    CHECK_ERROR(start_capture(&ctx), cleanup,
                "Failed to start capturing")

    CHECK_ERROR(stop_stream(&ctx), cleanup,
                "Failed to stop streaming");

    cleanup:
    if (ctx.cam_fd > 0)
        close(ctx.cam_fd);

    if (ctx.g_buff != NULL) {
        free(ctx.g_buff);
    }

    //NvBufferDestroy(ctx.render_dmabuf_fd);

    if (error)
        printf("App run failed\n");
    else
        printf("App run was successful\n");

    return -error;
}
